#pragma once
#include "Vector3.h"
#include "StateVector2.h"
#include "Matrix33.h"

class AccelerateFunctor
{
public:
	AccelerateFunctor(const Vector3<double>& acc)
	:mAcc(acc)
	{}
	~AccelerateFunctor(void) {};

	StateVector2<Vector3<double>,Vector3<double>> operator()(double t, StateVector2<Vector3<double>,Vector3<double>>& s) const
	{
		StateVector2<Vector3<double>,Vector3<double>> result(s);
		result.mS0 = s.mS1;
		result.mS1 = mAcc;
		return result;
	}

private:
	Vector3<double> mAcc;
};

class MassSpringDamperFunctor
{
public:
	MassSpringDamperFunctor(double c, double k, 
		double mass, const Vector3<double>& acc, const Vector3<double>& initPos) 
	:mC(c),mK(k),mMass(mass),
	mAcc(acc),mInitPos(initPos)
	{}
	~MassSpringDamperFunctor(void) {};

	StateVector2<Vector3<double>,Vector3<double>> operator()(double t, StateVector2<Vector3<double>,Vector3<double>>& s) const
	{
		StateVector2<Vector3<double>,Vector3<double>> result(s);
		result.mS0 = s.mS1;
		result.mS1 = ((mInitPos - s.mS0)*mK - s.mS1*mC)/mMass + mAcc;
		
		return result;
	}

private:
	double mC;
	double mK;
	double mMass;
	Vector3<double> mAcc;
	Vector3<double> mInitPos;
};

class RigidBodyFunctor
{
public:
	RigidBodyFunctor(const Matrix44<double>& inertia, const Matrix44<double>& rotation, const Vector3<double>& torque) 
		:mMatrixInertia(inertia),mMatrixRotation(rotation),mTorque(torque)
	{}
	~RigidBodyFunctor(void) {};

	StateVector2<Matrix44<double>,Vector3<double>> operator()(double t, StateVector2<Matrix44<double>,Vector3<double>>& s) const
	{
		StateVector2<Matrix44<double>,Vector3<double>> result(s);
		Matrix44<double> rT = mMatrixRotation;
		Vector3<double> w = mMatrixRotation*mMatrixInertia*rT.Transpose()*s.mS1;
		Matrix44<double> omega;omega.Identity();
		omega.m[1] = -w.z;omega.m[2] = w.y;
		omega.m[4] = w.z;omega.m[5] = 0;omega.m[6] = -w.x;
		omega.m[8] = -w.y;omega.m[9] = w.x;omega.m[10] = 0;omega.m[15] = 0;
		result.mS0 = s.mS0 + omega*mMatrixRotation*t;
		result.mS1 = s.mS1 + mTorque*t;

		return result;
	}

private:
	Matrix44<double> mMatrixInertia;
	Matrix44<double> mMatrixRotation;
	Vector3<double> mTorque;
};

// chaos theory
//result.x = 10*(s.y - s.x);
//result.y = (s.x*(28 - s.z) - s.y);
//result.z = s.x*s.y - 8.0*s.z/3;